from geometry_msgs.msg import PoseStamped #PoseStamped：ROS 2 的标准消息类型，用于表示带时间戳和坐标系的三维位姿（位置 + 方向）。
from nav2_simple_commander.robot_navigator import BasicNavigator #BasicNavigator：Nav2 提供的简化接口，封装了导航任务（如设置初始位姿、发送目标点）。
import rclpy

def main():
    rclpy.init()
    nav = BasicNavigator()    #创建 BasicNavigator 实例，它会自动启动 Nav2 的关键节点（如 controller_server、planner_server）。
    nav.waitUntilNav2Active()      #阻塞直到 Nav2 的所有服务启动完成
    #设定位置信息
    goal_pose = PoseStamped()                
    goal_pose.header.frame_id = "map"
    goal_pose.header.stamp = nav.get_clock().now().to_msg()
    goal_pose.pose.position.x =2.0
    goal_pose.pose.position.y =1.0
    goal_pose.pose.orientation.w =1.0
    nav.goToPose(goal_pose)  #将目标位姿发布
    while not nav.isTaskComplete():
        feedback = nav.getFeedback()
        nav.get_logger().info(f'剩余距离：{feedback.distance_remaining}')
        #nav.cancelTask() #可以取消任务，比如说超时取消
    result = nav.getResult()
    nav.get_logger().info(f'导航结果：{result}')

    #goToPose中有结束节点功能，所以不需要写后面两句
    #rclpy.spin(nav)
    #rclpy.shutdown()
